#include "path_generate/p2p/grid_node.h"

// 8邻域
const int GridNode::dir = 8;
const int GridNode::dx[] = { -1, -1, 0, 1, 1, 1, 0, -1 };
const int GridNode::dy[] = { 0, 1, 1, 1, 0, -1, -1, -1 };

// // 4邻域
// const int GridNode::dir = 4;
// const int GridNode::dx[] = { -1, 1, 0, 0};
// const int GridNode::dy[] = { 0, 0, -1, 1};

// 比较节点是否相同
bool GridNode::operator == (const GridNode& rhs) const {
  return x == rhs.x && y == rhs.y;
}

// 启发函数
double GridNode::movementCost(const GridNode& pred) const { 
  // // 计算欧几里得距离
  // double dx = this->x - pred.getX();
  // double dy = this->y - pred.getY();
  // return hypot(dx, dy);

  // 计算曼哈顿距离
  double dx = this->x - pred.getX();
  double dy = this->y - pred.getY();
  return abs(dx) + abs(dy);

  // // 计算切比雪夫距离
  // double dx = std::abs(this->x - pred.getX());
  // double dy = std::abs(this->y - pred.getY());
  // return std::max(dx, dy);
}
